#include <SoftwareSerial.h>
#Define other_bot 2
#Define me 1
#Define channel 200
#Define NULL=-1;
#Define RESEND=-2;
#Define PACKET=18

int deposition[][4]={{7,4,0,0},
		    {4,7,1,0},
                    {1,4,2,0},
	            {4,1,3,0}};
int chk[11][11];//0-central region can b covered,1-chkd central region pnts,4-chkd common cental region n obz poits,3-movement prohibited obz points,6-dep boxes,2-box location on cental reg5-boxes common to obz n cental 
int boxL[4][4];
int receiving,ans,chance,flip,x,y,flag,obz,dir,box,run_comp,task_comp,boxD,perm;//1 x 2 y 3 -x 4 -y,
int send[18],receive[18];
void setup()
{
Serial.begin(38400);

ans=1;flag=0;perm=0;boxD=4;flip=1;obz=0;box=0;dir=0;x=1;y=2;run_comp=0,task_comp;

for(int i=0;i<11;i++)
{
  	for(int j=0;j<11;j++)
        {
                if(i<7&&i>1&&j<7&&j>1)
                  chk[i][j]=0;
                else
                  chk[i][j]=3;
                if(i==10||j==10)
                  chk[i][j]=-1;  
        }         
}

chk[4][0]==chk[4][8]==chk[0][4]==chk[8][4]==6;                  

for(int i=12;i>7;i--)
    pinMode(i, INPUT);


send[0]=x;
send[1]=y;
send[2]=dir;
send[3]=chance;
send[4]=obz;
send[5]=run_comp;
send[6]=task_comp;
send[7]=box;
send[8]=boxD;
for(int i=9;i<17;i=i+2)
{
  send[i]=boxL[(i-9)][0];
  send[i+1]=boxL[(i-9)][1];
}
send[17]=NULL;
for(int i=0;i<18;i++)
  receive[i]=0;

}
void intrrupt()
{
  if(Serial.available()>0)
    update();
}

void update()
{
Serial.available()>0)
{
  int lst=Serial.read();
  if(lst==NULL)
  {
    for(int(i=16;i>=0;i--)
      receive[i]=Serial.read();
    x=receive[0];
    y=receive[1];
    dir=receive[2];
    chance=receive[3];
    obz=receive[4];
    run_comp=receive[5];
    task_comp=receive[6];
    box=receive[7];
    boxD=receive[8];
  for(int i=9;i<17;i=i+2)
  {
      boxL[(i-9)][0]=receive[i];
      boxL[(i-9)][1]=receive[i+1];
  }}
  else if(lst==RESEND)
    send();
  else
  Serial.print(RESEND);
  Serial.flush();
}

void send()
{

send[0]=x;
send[1]=y;
send[2]=dir;
send[3]=chance;
send[4]=obz;
send[5]=run_comp;
send[6]=task_comp;
send[7]=box;
send[8]=boxD;
for(int i=9;i<18;i=i+2)
{
  send[i]=boxL[(i-9)][0];
  send[i+1]=boxL[(i-9)][1];
}
send[17]=NULL;
for(int i=0;i<17;i++)
   Serial.print(send[i]);
   

}
int permitted()
{
    if(flag==1&&obz==0&&chance==1)
      return 1;
    else
      return 0;  
}
int pseudo_turn_l(int c)
{
    Serial.println("pseudo_turn_l()");
     if(c<3)
       c++;
     else
       c=0;
     return c;
}

void turn_l(void)
{
  Serial.println("turn_l()");    
  dir=pseudo_turn_l(dir);
  //turn motors with enough delay to exactly turn r   
}             		
  
int pseudo_turn_r(int c)
{        
                Serial.println("pseudo_turn_r()");
  
  		if(c>0)
                  c--;
                else
                  c=3;
                return c;
}

void turn_r(void)
{
    Serial.println("turn_r()");
   dir=pseudo_turn_r(dir);
   //turn motors to right exactly l
}

void back(void)
{
     dir=pseudo_turn_r(pseudo_turn_r(dir));
     pseudo_change(dir,1);
     dir=pseudo_turn_r(pseudo_turn_r(dir));
}
int pseudo_change(int c,int d)
{
  Serial.println("pseudo_change()");
  if(c==0)
    x++;
  if(c==1)
    y++;
  if(c==2)
    x--;
  if(c==3)
    y--;
  if(permitted()==0&&x==7&&y==2)
  {
     back();
     return -1;
  }  
 if(d==0)
    back(); 
  return 1;
}
void change(void)
{
  Serial.println("change()");
  int v=pseudo_change(dir,1);
  for(int i=x;i<x+3;i++)
    for(int j=y;j<y+3;j++)
    {    if(chk[i][j]==0)
          chk[i][j]=1;
        if(chk[i][j]==3)
          chk[i][j]=4;  
    }
}
void turn_accord()
{
  Serial.println("turn accord()");  
  if(pseudo_change(pseudo_turn_l(dir),0)==1)
        turn_l();
    else 
        turn_r();
    /*if(digitalRead(8)==0&&digitalRead(10)==0) 
    {
      //motor bkwards for 1 grid point
        turn_accord();    
    }*/
}
void chkobj()
{
           Serial.println("ckhobj()");
          int f;
         /*if(digitalRead(10)==0)
             f=recL(10);
            
         if(digitalRead(8)==0) 
             f=recL(8);*/   
      /*   if(digitalRead(9)==0)
           if(x==6&&y==2&&run_comp==0)
             f=recL(9);
           else
             found(9);*/
}
void avoid()
{
}
void pick()
{
    flag=1;
    //motor fwd for 1
    //change();
    //chkobj();
}
void drop()
{
    flag=0;
    chance=2;
}
double dis(int a,int b,int c,int d)
{
    return sqrt((a-c)*(a-c)+(b-d)*(b-d));
}

int nearest(int chkarr[][4],int size)
{
  double arr[4];
  int m;
  for(int i=0;i<size;i++)
  {
      arr[i]=dis(chkarr[i][0],chkarr[i][1],x,y);
      if(chkarr[i][3]==1)
        arr[i]=100.0;
  }
  m=0;
  for(int i=0;i<size;i++)
    if(arr[i]<arr[m])
      m=i;
  deposition[m][3]=1;
  return m;
}    

void gotoxy(int a,int b,int di)
{
    int movex,movey;
    movex=(a-x);
    movey=(b-y);
    while(movex!=0||movey!=0)
    {
        if((dir==0&&movex>0)||(dir==2&&movex<0))
        {
            move(abs(movex));
            movex=0;
        }
        if((dir==1&&movey>0)||(dir==3&&movey<0))
        {
            move(abs(movey));
            movey=0;
        }
        if((dir==1&&movex<0)||(dir==0&&movey>0)||(dir==3&&movex>0)||(dir==2&&movey<0))
            turn_l();
        else
            turn_r();
    }
    if(di>=0&&di<=3)
    {
      while(di!=dir)
      {
        if(dir==0&&di==3)
            {
                turn_r();
                continue;  
            }
        if(dir==3&&di==0)
            {
                turn_l();
                continue;
            } 
        if(di<dir)
            turn_r();
        if(di>dir)
            turn_l();
      }  
    }
}
int recL(int sensor)
{
    Serial.println("  recL()  ");
  int x0,y0,dir0;  
  int i,yc,xc,c,currbox;
    currbox=-1;
    x0=x;
    y0=y;
    dir0=dir;
    c=dir;
    if(sensor==10)
       c=pseudo_turn_l(dir);
    else if(sensor==8)
       c=pseudo_turn_r(dir);
    
    int v=pseudo_change(c,1);
    xc=x;
    yc=y;
    x=x0;
    y=y0;
    dir=dir0;
    for(i=0;i<box;i++)
    {
        if(boxL[i][0]==xc&&boxL[i][1]==yc)
        {    
            currbox=i;
            break;
        }
    }
    if(i==box)
    {
        currbox=i;
        chk[xc+1][yc+1]=chk[xc+1][yc+1]+1;
        boxL[box][0]=xc;
        boxL[box][1]=yc;
        if(sensor==9)
            boxL[box][2]=-1;
        else    
            boxL[box][2]=nearest(deposition,4);
        box++;
        Serial.print(boxL[box-1][0]);
        Serial.print(boxL[box-1][1]);
        
    }
    
    return currbox;
}

void found(int sensor)
{
    int x0,y0,dir0,c,c1,currbox;
    c=dir;
    currbox=recL(sensor);
    if(sensor==9&&run_comp==0)
        flip=flip*(-1);
    if(flag==0&&run_comp==1||(flag==0&&sensor==9&&flip==-1))
    {
      Serial.println("Picking");  
      if(sensor==10&&run_comp==1)
            turn_l();
        if(sensor==8&&run_comp==1)
            turn_r();    
        pick();
    }
    if((flag==1&&run_comp==1)||(flag==1&&run_comp==0&&flip==1))
    {
         
   Serial.println("Drop");
       if(run_comp==0)
            {  
              dir0=dir;
              y0=y;
              x0=x;
            //  c1=nearest(deposition,4);
            }
            else{}
              //  c1=boxL[currbox][2];
          if(boxD==4)
            {
              chance=1;
             Serial.println("Meri Chance"); 
            }
            boxD--;
           
            //gotoxy(deposition[c1][0],deposition[c1][1],deposition[c1][2]);
            Serial.println("dropping");
            drop();
            currbox=-1;
            if(run_comp=1)
            {
                  c1=nearest(boxL,box);
                  x0=boxL[c1][0];y0=boxL[c1][1];dir0=boxL[c1][2];
            }

            //gotoxy(x0,y0,dir0);
  }
     
    
}  
void move(int steps)
{
  int f=-1;
  while(steps!=0)
  {
    //motors frwd
    if(digitalRead(12)==0&&digitalRead(11)==0)
    {
       change(); 
       for(int i=8;i<11;i++)
       if(digitalRead(i)==0)
         if(flag==1)
             f=recL(i);
         else
             found(i);    
     steps--;
     while(pseudo_change(dir,0)==-1)
       turn_accord();
     
   }
       
  if(digitalRead(12)==0){}
     //motor right frwd lft bkwd;  
  if(digitalRead(11)==0){}
    //motor lft frwd right bkwd;
 }
}





void loop()
{
  int f,c;
  //motors frwd
   digitalWrite(3,LOW);
   digitalWrite(11,LOW);
digitalWrite(9,HIGH);
     digitalWrite(10,HIGH);
  if(digitalRead(12)==1/*&&digitalRead(11)==1*/&&ans==0)
      {Serial.println("1");ans=1;
    delay(50);}
  if(digitalRead(12)==0/*&&digitalRead(11)==0*/&&ans==1)
  {
 /*    digitalWrite(3,LOW);
   digitalWrite(11,LOW);
digitalWrite(9,LOW);
     digitalWrite(10,LOW);*/
    Serial.println("2");ans=0;delay(50);
    
    Serial.println("grid"); 
    change();
     Serial.println(" ");
     Serial.print(x);
     Serial.print("  ");
     Serial.print(y);
       Serial.print("  ");
     Serial.println(dir);
     
     if((x==6&&y==6)||(box==4))
     {
         Serial.println("run_comp");  
       run_comp=1;
         chkobj();
         for(int i=0;i<11;i++)
	{for(int j=0;j<11;j++)
          Serial.print(chk[i][j]);
          Serial.println();}
         if(flag==1)
         {
             f=nearest(deposition,box);
             gotoxy(deposition[f][0],deposition[f][1],deposition[f][2]);
             drop();         
         }
     }
     if(run_comp==0)
       chkobj();     
     while(pseudo_change(dir,0)==-1)
     {  
       Serial.println("invalid");
       turn_l();
      }
  /*   if(run_comp==1)
     {
         c=nearest(boxL,box);
         gotoxy(boxL[c][0],boxL[c][1],boxL[c][2]);
     }*/
  }
  if(digitalRead(12)==0){}
     //motor right frwd lft bkwd; BASIC LINE FOLLOWER 
  if(digitalRead(11)==0){}
    //motor lft frwd right bkwd;
  
}


